/***
*
*   █████▒█    ██  ▄████▄   ██ ▄█▀       ██████╗ ██╗   ██╗ ██████╗
* ▓██   ▒ ██  ▓██▒▒██▀ ▀█   ██▄█▒        ██╔══██╗██║   ██║██╔════╝
* ▒████ ░▓██  ▒██░▒▓█    ▄ ▓███▄░        ██████╔╝██║   ██║██║  ███╗
* ░▓█▒  ░▓▓█  ░██░▒▓▓▄ ▄██▒▓██ █▄        ██╔══██╗██║   ██║██║   ██║
* ░▒█░   ▒▒█████▓ ▒ ▓███▀ ░▒██▒ █▄       ██████╔╝╚██████╔╝╚██████╔╝
*  ▒ ░   ░▒▓▒ ▒ ▒ ░ ░▒ ▒  ░▒ ▒▒ ▓▒       ╚═════╝  ╚═════╝  ╚═════╝
*  ░     ░░▒░ ░ ░   ░  ▒   ░ ░▒ ▒░
*  ░ ░    ░░░ ░ ░ ░        ░ ░░ ░
*           ░     ░ ░      ░  ░
*
* @Date: 2024-04-07 20:40:50
* @LastEditors: KraHsu && 1191393280@qq.com
* @LastEditTime: 2024-04-07 20:40:50
* Copyright (c) 2024 by KraHsu, All Rights Reserved.
*/

#pragma once

#include <cstddef>
#include <algorithm>
#include "cmsis_compiler.h"

namespace os::math {
constexpr uint16_t RoundUpToPowerOfTwo(uint16_t x) {
    if ((x & (x - 1)) == 0)
        return x;
    x--;
    x |= x >> 1;
    x |= x >> 2;
    x |= x >> 4;
    x |= x >> 8;
    x++;
    return x;
}

template <typename T, uint16_t LenT>
class RingBuffer {
    static_assert(LenT >= 2, "Length must be greater than or equal to 2");
    static_assert(LenT <= 2048, "Length must be less than or equal to 2048");

public:
    static constexpr uint16_t kCapacity = RoundUpToPowerOfTwo(LenT);
    static_assert(kCapacity * sizeof(T) <= 2048, "Size must be less than or equal to 2048");

private:
    uint16_t in_{0};
    uint16_t out_{0};
    T buffer_[kCapacity];

public:
    RingBuffer() = default;

    uint16_t Write(const T* buffer, uint16_t len) {
        len = std::min(len, static_cast<uint16_t>(kCapacity - in_ + out_));

        __DMB();

        uint16_t l = std::min(len, static_cast<uint16_t>(kCapacity - (in_ & (kCapacity - 1))));
        memcpy(buffer_ + (in_ & (kCapacity - 1)), buffer, l * sizeof(T));

        memcpy(buffer_, buffer + l, (len - l) * sizeof(T));

        __DMB();

        in_ += len;

        return len;
    }

    uint16_t Write(const T& in) {
        return Write(&in, 1);
    }

    uint16_t Read(T* buffer, uint16_t len) {
        len = std::min(len, static_cast<uint16_t>(in_ - out_));

        __DMB();

        uint16_t l = std::min(len, static_cast<uint16_t>(kCapacity - (out_ & (kCapacity - 1))));
        memcpy(buffer, buffer_ + (out_ & (kCapacity - 1)), l * sizeof(T));

        memcpy(buffer + l, buffer_, (len - l) * sizeof(T));

        __DMB();

        out_ += len;

        return len;
    }

    T Read() {
        T res;
        Read(&res, 1);
        return std::move(res);
    }

    uint16_t Read(T& out) {
        return Read(&out, 1);
    }

    T operator[](uint16_t i) {
        return buffer_[(in_ + i) & (kCapacity - 1)];
    }

    RingBuffer& operator<<(const T& in) {
        Write(in);
        return *this;
    }

    RingBuffer& operator>>(T& out) {
        Read(out);
        return *this;
    }
};

template <typename T, size_t LenT>
class SlidingWindow {
private:
    RingBuffer<T, LenT> ring_buffer_;

public:
    explicit SlidingWindow(const T& init_value) {
        for (size_t i = 0; i < LenT; i++) {
            ring_buffer_.Write(init_value);
        }
    }

    explicit SlidingWindow() {
        T t{};
        for (size_t i = 0; i < LenT; i++) {
            ring_buffer_.Write(t);
        }
    }

    SlidingWindow& operator<<(const T& in) {
        ring_buffer_.Write(in);
        ring_buffer_.Read();
        return *this;
    }

    T operator[](size_t i) {
        return ring_buffer_[i];
    }
};
} // namespace os::math